#include <Arduino.h>
#include <SimpleFOC.h>

// Example of AS5600 configuration

MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
TwoWire I2Ctwo = TwoWire(1);

void setup()
{
    // monitoring port
    Serial.begin(115200);
    I2Ctwo.begin(37, 36, 400000UL);

    // initialise magnetic sensor hardware
    sensor.init(&I2Ctwo);

    Serial.println("Sensor ready");
    _delay(1000);
}

void loop()
{
    // iterative function updating the sensor internal variables
    // it is usually called in motor.loopFOC()
    // this function reads the sensor hardware and
    // has to be called before getAngle nad getVelocity
    sensor.update();

    // display the angle and the angular velocity to the terminal
    Serial.print(sensor.getAngle());
    Serial.print("\t");
    Serial.println(sensor.getVelocity());
    // delay(1000);
}
